/*************************************************************
*
* VOXEL.CPP
*
* Generic voxel support ('vox')
*************************************************************/
#include <stdafx.h>

//#include <ap_comm.h>
#include <m3d/m3d.h>


/**********************************************************
* voxGRID::GetNVoxels()
*
* Compute the total number of voxels in a grid
**********************************************************/
int voxGRID::GetNVoxels (void) const
{
   return this->nVoxel.x*this->nVoxel.y*this->nVoxel.z;
}

/*************************************************************
* voxGRID::GetPosInd()
*
* Get voxel indices of a point
*************************************************************/
void voxGRID::GetPosInd (const m3dV *pos, int *x, int *y, int *z, BOOL isClamp /*= TRUE*/) const
{
   const m3dBOX *pBB = &this->bbox;

   if (x) {
      *x = (int)((pos->x - pBB->bll.x - M3D_EPSILON) / GetVoxSizeX());
   }
   if (y) {
      // for cdtLOCAL, 'y' may be not of interest
      *y = (int)((pos->y - pBB->bll.y - M3D_EPSILON) / GetVoxSizeY());
   }
   if (z) {
      *z = (int)((pos->z - pBB->bll.z - M3D_EPSILON) / GetVoxSizeZ());
   }
   if (isClamp) {
      ClampIndices(x, y, z);
   }
   return;
}

/*************************************************************
* voxGRID::BuildNewGridUsingVoxel()
*
*************************************************************/
void  voxGRID::BuildNewGridUsingVoxel (int x, int y, int z, voxGRID *pB, int nx, int ny, int nz)
{
   ASSERT(x >= 0 && x < GetNVoxelX());
   ASSERT(y >= 0 && y < GetNVoxelY());
   ASSERT(z >= 0 && z < GetNVoxelZ());
   pB->bbox.bll.x = bbox.bll.x + x*GetVoxSizeX();
   pB->bbox.bll.y = bbox.bll.y + y*GetVoxSizeY();
   pB->bbox.bll.z = bbox.bll.z + z*GetVoxSizeZ();
   pB->bbox.fur.x = pB->bbox.bll.x + GetVoxSizeX();
   pB->bbox.fur.y = pB->bbox.bll.y + GetVoxSizeY();
   pB->bbox.fur.z = pB->bbox.bll.z + GetVoxSizeZ();
   return;
}

/*************************************************************
* voxGRID::ClampIndices()
*
* Clamp the voxel indices to the valid range
*************************************************************/
void voxGRID::ClampIndices (int *x, int *y, int *z) const
{
   if (x) {
      *x = _m3dClampT(0, GetNVoxelX()-1, *x);
   }
   if (y) {
      // for cdtLOCAL, 'y' may be not of interest
      *y = _m3dClampT(0, GetNVoxelY()-1, *y);
   }
   if (z) {
      *z = _m3dClampT(0, GetNVoxelZ()-1, *z);
   }
   return;
}

/*************************************************************
* voxGRID::IsValidInd()
*
* IN : x,y,z - voxel indices
* RET: TRUE if voxel indices are all valid
* Check if the three indices are valid voxel indices
*************************************************************/
BOOL voxGRID::IsValidInd (int x, int y, int z) const
{
   if (x < 0 || x >= GetNVoxelX()) {
      return FALSE;
   }
   if (y < 0 || y >= GetNVoxelY()) {
      return FALSE;
   }
   if (z < 0 || z >= GetNVoxelZ()) {
      return FALSE;
   }
   return TRUE;
}

/*************************************************************
* voxGRID::CalcVoxSize()
*
* OUT: this->voxSize
*
* Assuming that bbox and nVoxel are already computed,
* compute voxel size (voxSize)
*************************************************************/
BOOL voxGRID::CalcVoxSize (void)
{
   m3dV    size;
   BOOL    rc = TRUE;
   m3dBOX *pBB = &this->bbox;

   // Check number of voxels
   if (GetNVoxelX() <= 0 || GetNVoxelY() <= 0 || GetNVoxelZ() <= 0) {
      ASSERT(FALSE);
      return FALSE;
   }
   if (m3dIsZero(pBB->fur.x - pBB->bll.x) ||
       m3dIsZero(pBB->fur.y - pBB->bll.y) ||
       m3dIsZero(pBB->fur.z - pBB->bll.z)) {
      rc = FALSE;
   }
   m3dSubtractVector(&pBB->fur, &pBB->bll, &size);
   this->voxSize.x = size.x / GetNVoxelX();
   this->voxSize.y = size.y / GetNVoxelY();
   this->voxSize.z = size.z / GetNVoxelZ();
   return rc;
}

/*************************************************************
* voxGRID::CalcNVoxel()
*
* OUT: this->nVoxel
*
* Assuming that bbox and voxel size (voxSize) are already computed,
* compute number of voxels (nVoxel) along each axis
*************************************************************/
void voxGRID::CalcNVoxel (void)
{
   m3dBOX *pBB = &this->bbox;

   this->nVoxel.x = voxCalcNVoxel(pBB->fur.x - pBB->bll.x, GetVoxSizeX());
   this->nVoxel.y = voxCalcNVoxel(pBB->fur.y - pBB->bll.y, GetVoxSizeY());
   this->nVoxel.z = voxCalcNVoxel(pBB->fur.z - pBB->bll.z, GetVoxSizeZ());
   return;
}

/*************************************************************
* voxCalcNVoxel()
*
* IN : sizeBBoxSide - size of voxGrid along certain axis
*      sizeVox      - size of a voxel along this axis
* RET: number of voxels along this axis
*
* Compute number of voxels along an axis
*************************************************************/
int voxCalcNVoxel (float sizeBBoxSide, float sizeVox)
{
   return (int)(sizeBBoxSide / sizeVox) + 1;
}

/*************************************************************
* voxGRID::GetPosLinearInd()
*
* IN : pos - point in 3D
* RET: linear index of a point
*************************************************************/
int voxGRID::GetPosLinearInd (m3dV *pos)
{
   int x, y, z;

   GetPosInd(pos, &x, &y, &z);
   return GetLinearInd(x, y, z);
}

/*************************************************************
* voxGRID::LinearInd2Ind()
*
* IN: linInd  - linear index
*     x, y, z - 3D indices
*
* Compute (x, y, z) indices of the voxel specified
* by voxel's linear index
*************************************************************/
void voxGRID::LinearInd2Ind (int linInd, int *x, int *y, int *z)
{
   int l;
   int _x, _y, _z;

   _x = linInd / (GetNVoxelY()*GetNVoxelZ());
   l = linInd - _x*GetNVoxelY()*GetNVoxelZ();

   _y = l / GetNVoxelZ();
   l = l - _y*GetNVoxelZ();
   
   _z = l;

   if (x) {
      *x = _x;
   }
   if (y) {
      *y = _y;
   }
   if (z) {
      *z = _z;
   }

   ASSERT(GetLinearInd(*x, *y, *z) == linInd);
   return;
}

/*************************************************************
* voxGRID::CalcVoxBBox()
*
* IN  : x,y,z  - indices of voxel in the grid
* OUT : pBB    - voxel's bounding box
*************************************************************/
void voxGRID::CalcVoxBBox (int x, int y, int z, m3dBOX *pBB)
{
   pBB->bll.x = this->bbox.bll.x + GetVoxSizeX()*x;
   pBB->bll.y = this->bbox.bll.y + GetVoxSizeY()*y;
   pBB->bll.z = this->bbox.bll.z + GetVoxSizeZ()*z;

   pBB->fur.x = pBB->bll.x + GetVoxSizeX();
   pBB->fur.y = pBB->bll.y + GetVoxSizeY();
   pBB->fur.z = pBB->bll.z + GetVoxSizeZ();
   return;
}


/*************************************************************
* voxITER_BOX_SYMMETRICAL
*************************************************************/
/*************************************************************
* voxITER_BOX_SYMMETRICAL::ApplyCS ()
*
*************************************************************/
BOOL voxITER_BOX_SYMMETRICAL::SetBox  (m3dBOX *pBB) 
{ 
   // don't clip box by box here - it's impossible for this iterator 
   // because it's special for sphere and alpha cut
   box = *pBB;
   return TRUE;
}

/*************************************************************
* voxITER_BOX_SYMMETRICAL::ApplyCS ()
*
*************************************************************/
void voxITER_BOX_SYMMETRICAL::ApplyCS (m3dMATR *pMatrW2L) 
{
   m3dOBB   obb;
   box.Transform(pMatrW2L, &obb);
   obb.CalcEnclBox(&box);
   return;
}
/*************************************************************
* voxITER_BOX_SYMMETRICAL::Start ()
*
* Start iterator of indices (of voxels), which can be touched 
* while the moving from vFrom to vTo.
*************************************************************/
BOOL voxITER_BOX_SYMMETRICAL::Start (void)
{
   // don't clip box here
   if (!_m3dCheckValid(&box.bll) || !_m3dCheckValid(&box.fur)) {
      STRONG_ASSERT(FALSE);
      return FALSE;
   }

   pVG->GetPosInd(&box.bll, &from.x, &from.y, &from.z);
   pVG->GetPosInd(&box.fur, &to.x,   &to.y,   &to.z);
   m3dV     center;
   box.GetCenter(&center);

   pVG->GetPosInd(&center, &fromNext.x, &fromNext.y, &fromNext.z, FALSE);
   toNext = fromNext;
   // this ASSERT is wrong for this iterator
   //ASSERT(fromNext.x>=from.x && fromNext.x<=to.x && fromNext.y>=from.y && fromNext.y<=to.y && fromNext.z>=from.z && fromNext.z<=to.z);

   m3dBOX   bb;
   pVG->CalcVoxBBox(fromNext.x, fromNext.y, fromNext.z, &bb);
   distCur = M3D_INFINITY;
   step    = M3D_INFINITY;
   if (pVG->nVoxel.x > 1) {
      distCur = min(distCur, min(center.x - bb.bll.x, bb.fur.x - center.x));
      step    = min(step,    pVG->voxSize.x);
   }
   if (pVG->nVoxel.y > 1) {
      distCur = min(distCur, min(center.y - bb.bll.y, bb.fur.y - center.y));
      step    = min(step,    pVG->voxSize.y);
   }
   if (pVG->nVoxel.z > 1) {
      distCur = min(distCur, min(center.z - bb.bll.z, bb.fur.z - center.z));
      step    = min(step,    pVG->voxSize.z);
   }
   if (distCur > M3D_INFINITY/2) {
      distCur = 0.f;
      step    = min(min(pVG->voxSize.x, pVG->voxSize.y), pVG->voxSize.z);
   }
   return TRUE;
}
/*************************************************************
* voxITER_BOX_SYMMETRICAL::Next ()
*
*************************************************************/
BOOL voxITER_BOX_SYMMETRICAL::Next (int *indList, int *nInd, int indNumMax)
{
   m3dVINT   _fromCur, _toCur, _from, _to;
   int       x, y, z;

   ASSERT(pVG->IsValidInd(from.x,   from.y,   from.z));
   ASSERT(pVG->IsValidInd(to.x,     to.y,     to.z));

   if (fromNext.x < from.x && fromNext.y < from.y && fromNext.z < from.z &&
       toNext.x   > to.x   && toNext.y   > to.y   && toNext.z   > to.z) {
      return FALSE;
   }


   *nInd = 0;

   _fromCur = fromNext;
   _toCur   = toNext;

   // y const
   _from.x = max(_fromCur.x, from.x);
   _to.x   = min(_toCur.x,   to.x);
   _from.z = max(_fromCur.z, from.z);
   _to.z   = min(_toCur.z,   to.z);
   if (_fromCur.y >= from.y && _fromCur.y <= to.y) {
      for (x = _from.x; x <= _to.x; x ++) {
         for (z = _from.z; z <= _to.z; z ++) {
            if (*nInd >= indNumMax) {
               ASSERT(FALSE);
               goto exit;
            }
            indList[(*nInd) ++] = pVG->GetLinearInd(x, _fromCur.y, z);
         }
      }
   }
   if (_toCur.y >= from.y && _toCur.y <= to.y && _fromCur.y != _toCur.y) {
      for (x = _from.x; x <= _to.x; x ++) {
         for (z = _from.z; z <= _to.z; z ++) {
            if (*nInd >= indNumMax) {
               ASSERT(FALSE);
               goto exit;
            }
            indList[(*nInd) ++] = pVG->GetLinearInd(x, _toCur.y, z);
         }
      }
   }
   _fromCur.y ++; //all voxels from this slice were added
   _toCur.y   --;

   // x const
   _from.y = max(_fromCur.y, from.y);
   _to.y   = min(_toCur.y,   to.y);
   if (_fromCur.x >= from.x && _fromCur.x <= to.x) {
      for (y = _from.y; y <= _to.y; y ++) {
         for (z = _from.z; z <= _to.z; z ++) {
            if (*nInd >= indNumMax) {
               ASSERT(FALSE);
               goto exit;
            }
            indList[(*nInd) ++] = pVG->GetLinearInd(_fromCur.x, y, z);
         }
      }
   }
   if (_toCur.x >= from.x && _toCur.x <= to.x && _fromCur.x != _toCur.x) {
      for (y = _from.y; y <= _to.y; y ++) {
         for (z = _from.z; z <= _to.z; z ++) {
            if (*nInd >= indNumMax) {
               ASSERT(FALSE);
               goto exit;
            }
            indList[(*nInd) ++] = pVG->GetLinearInd(_toCur.x, y, z);
         }
      }
   }
   _fromCur.x ++;
   _toCur.x   --;

   // z const
   _from.x = max(_fromCur.x, from.x);
   _to.x   = min(_toCur.x,   to.x);
   if (_fromCur.z >= from.z && _fromCur.z <= to.z) {
      for (x = _from.x; x <= _to.x; x ++) {
         for (y = _from.y; y <= _to.y; y ++) {
            if (*nInd >= indNumMax) {
               ASSERT(FALSE);
               goto exit;
            }
            indList[(*nInd) ++] = pVG->GetLinearInd(x, y, _fromCur.z);
         }
      }
   }
   if (_toCur.z >= from.z && _toCur.z <= to.z && _fromCur.z != _toCur.z) {
      for (x = _from.x; x <= _to.x; x ++) {
         for (y = _from.y; y <= _to.y; y ++) {
            if (*nInd >= indNumMax) {
               ASSERT(FALSE);
               goto exit;
            }
            indList[(*nInd) ++] = pVG->GetLinearInd(x, y, _toCur.z);
         }
      }
   }

exit:
   fromNext.x --;
   fromNext.y --;
   fromNext.z --;
   toNext.x   ++;
   toNext.y   ++;
   toNext.z   ++;
   distCur    += step;

   return TRUE;
}

/*************************************************************
* voxITER_BOX
*************************************************************/
/*************************************************************
* voxITER_BOX::ApplyCS ()
*
*************************************************************/
BOOL voxITER_BOX::SetBox  (const m3dBOX *pBB) 
{ 
   // don't clip box by box here - will do it at voxITER_BOX::Start
   box = *pBB;
   return TRUE;
}

/*************************************************************
* voxITER_BOX::ApplyCS ()
*
*************************************************************/
void voxITER_BOX::ApplyCS (m3dMATR *pMatrW2L) 
{
   m3dOBB   obb;
   box.Transform(pMatrW2L, &obb);
   obb.CalcEnclBox(&box);
   return;
}
/*************************************************************
* voxITER_BOX::Start ()
*
* Start iterator of indices (of voxels), which can be touched 
* while the moving from vFrom to vTo.
*************************************************************/
BOOL voxITER_BOX::Start (void)
{
   // flat box, no iteration
   // b/c in GetPosInd will be division by zero
   if (pVG->GetVoxSizeX() < M3D_EPSILON || pVG->GetVoxSizeY() < M3D_EPSILON || pVG->GetVoxSizeZ() < M3D_EPSILON) {
      return FALSE;
   }

   m3dBOX boxIsect;
   if (pVG->bbox.ClipBox(box, &boxIsect) == M3D_OUTSIDE) {
      return FALSE;
   }
   box = boxIsect;

   if (!_m3dCheckValid(&box.bll) || !_m3dCheckValid(&box.fur)) {
      STRONG_ASSERT(FALSE);
      return FALSE;
   }

   pVG->GetPosInd(&box.bll, &from.x, &from.y, &from.z);
   pVG->GetPosInd(&box.fur, &to.x,   &to.y,   &to.z);
   m3dV     center;
   box.GetCenter(&center);

   pVG->GetPosInd(&center, &fromNext.x, &fromNext.y, &fromNext.z, FALSE);
   toNext = fromNext;
   ASSERT(fromNext.x>=from.x && fromNext.x<=to.x && fromNext.y>=from.y && fromNext.y<=to.y && fromNext.z>=from.z && fromNext.z<=to.z);

   m3dBOX   bb;
   pVG->CalcVoxBBox(fromNext.x, fromNext.y, fromNext.z, &bb);
   distCur = M3D_INFINITY;
   step    = M3D_INFINITY;
   if (pVG->nVoxel.x > 1) {
      distCur = min(distCur, min(center.x - bb.bll.x, bb.fur.x - center.x));
      step    = min(step,    pVG->voxSize.x);
   }
   if (pVG->nVoxel.y > 1) {
      distCur = min(distCur, min(center.y - bb.bll.y, bb.fur.y - center.y));
      step    = min(step,    pVG->voxSize.y);
   }
   if (pVG->nVoxel.z > 1) {
      distCur = min(distCur, min(center.z - bb.bll.z, bb.fur.z - center.z));
      step    = min(step,    pVG->voxSize.z);
   }
   if (distCur > M3D_INFINITY/2) {
      distCur = 0.f;
      step    = min(min(pVG->voxSize.x, pVG->voxSize.y), pVG->voxSize.z);
   }
   return TRUE;
}
/*************************************************************
* voxITER_BOX::Next ()
*
*************************************************************/
BOOL voxITER_BOX::Next (int *indList, int *nInd, int indNumMax)
{
   m3dVINT   _fromCur, _toCur, _from, _to;
   int       x, y, z;

   ASSERT(pVG->IsValidInd(from.x,   from.y,   from.z));
   ASSERT(pVG->IsValidInd(to.x,     to.y,     to.z));

   if (fromNext.x < from.x && fromNext.y < from.y && fromNext.z < from.z &&
      toNext.x   > to.x   && toNext.y   > to.y   && toNext.z   > to.z) {
         return FALSE;
      }


      *nInd = 0;

      _fromCur = fromNext;
      _toCur   = toNext;

      // y const
      _from.x = max(_fromCur.x, from.x);
      _to.x   = min(_toCur.x,   to.x);
      _from.z = max(_fromCur.z, from.z);
      _to.z   = min(_toCur.z,   to.z);
      if (_fromCur.y >= from.y && _fromCur.y <= to.y) {
         for (x = _from.x; x <= _to.x; x ++) {
            for (z = _from.z; z <= _to.z; z ++) {
               if (*nInd >= indNumMax) {
                  ASSERT(FALSE);
                  goto exit;
               }
               indList[(*nInd) ++] = pVG->GetLinearInd(x, _fromCur.y, z);
            }
         }
      }
      if (_toCur.y >= from.y && _toCur.y <= to.y && _fromCur.y != _toCur.y) {
         for (x = _from.x; x <= _to.x; x ++) {
            for (z = _from.z; z <= _to.z; z ++) {
               if (*nInd >= indNumMax) {
                  ASSERT(FALSE);
                  goto exit;
               }
               indList[(*nInd) ++] = pVG->GetLinearInd(x, _toCur.y, z);
            }
         }
      }
      _fromCur.y ++; //all voxels from this slice were added
      _toCur.y   --;

      // x const
      _from.y = max(_fromCur.y, from.y);
      _to.y   = min(_toCur.y,   to.y);
      if (_fromCur.x >= from.x && _fromCur.x <= to.x) {
         for (y = _from.y; y <= _to.y; y ++) {
            for (z = _from.z; z <= _to.z; z ++) {
               if (*nInd >= indNumMax) {
                  ASSERT(FALSE);
                  goto exit;
               }
               indList[(*nInd) ++] = pVG->GetLinearInd(_fromCur.x, y, z);
            }
         }
      }
      if (_toCur.x >= from.x && _toCur.x <= to.x && _fromCur.x != _toCur.x) {
         for (y = _from.y; y <= _to.y; y ++) {
            for (z = _from.z; z <= _to.z; z ++) {
               if (*nInd >= indNumMax) {
                  ASSERT(FALSE);
                  goto exit;
               }
               indList[(*nInd) ++] = pVG->GetLinearInd(_toCur.x, y, z);
            }
         }
      }
      _fromCur.x ++;
      _toCur.x   --;

      // z const
      _from.x = max(_fromCur.x, from.x);
      _to.x   = min(_toCur.x,   to.x);
      if (_fromCur.z >= from.z && _fromCur.z <= to.z) {
         for (x = _from.x; x <= _to.x; x ++) {
            for (y = _from.y; y <= _to.y; y ++) {
               if (*nInd >= indNumMax) {
                  ASSERT(FALSE);
                  goto exit;
               }
               indList[(*nInd) ++] = pVG->GetLinearInd(x, y, _fromCur.z);
            }
         }
      }
      if (_toCur.z >= from.z && _toCur.z <= to.z && _fromCur.z != _toCur.z) {
         for (x = _from.x; x <= _to.x; x ++) {
            for (y = _from.y; y <= _to.y; y ++) {
               if (*nInd >= indNumMax) {
                  ASSERT(FALSE);
                  goto exit;
               }
               indList[(*nInd) ++] = pVG->GetLinearInd(x, y, _toCur.z);
            }
         }
      }

exit:
      fromNext.x --;
      fromNext.y --;
      fromNext.z --;
      toNext.x   ++;
      toNext.y   ++;
      toNext.z   ++;
      distCur    += step;

      return TRUE;
}

/*************************************************************
* voxITER_SPH
*************************************************************/
/*************************************************************
* voxITER_SPH::ApplyCS ()
*
*************************************************************/
void voxITER_SPH::ApplyCS (m3dMATR *pMatrW2L) 
{
   m3dV   center;
   float  radius;
   
   box.GetCenter(&center);
   radius = 0.5f*(box.fur.x - box.bll.x);
   pMatrW2L->TransformPoint(&center);
   box.MakePointPoint(&center, &center);
   box.Expand(radius);
   return;
}
/*************************************************************
* voxITER_RAY
*************************************************************/
/*************************************************************
* voxITER_RAY::SetRay ()
*
*************************************************************/
BOOL voxITER_RAY::SetRay(m3dV *_vFrom, m3dV *_vTo) 
{ 
   // don't clip ray by box here - will do it at voxITER_RAY::Start
   vFrom = *_vFrom;
   vTo   = *_vTo;
   return TRUE;
}

/*************************************************************
* voxITER_RAY::ApplyCS ()
*
*************************************************************/
void voxITER_RAY::ApplyCS (m3dMATR *pMatrW2L) 
{
   pMatrW2L->TransformPoint(&vFrom);
   pMatrW2L->TransformPoint(&vTo);
   return;
}
/*************************************************************
* voxITER_RAY::Start ()
*
* Start iterator of indices (of voxels), which can be touched 
* while the moving from vFrom to vTo.
*************************************************************/
BOOL voxITER_RAY::Start (void)
{
   m3dV    vGFrom, vGTo;        // start and finish of linear segment in voxel grid LCS

   if (!_m3dCheckValid(&vFrom) || !_m3dCheckValid(&vTo)) {
      STRONG_ASSERT(FALSE);
      return FALSE;
   }

   // BBOX test
   if (!pVG->bbox.ClipLSeg(&vFrom, &vTo, &vGFrom, &vGTo)) {
      return FALSE;
   }

   // Transforms vFrom and vTo to voxel grid CS (in floats!)
   vGFrom.x = (vGFrom.x - pVG->bbox.bll.x) / pVG->GetVoxSizeX();
   vGFrom.y = (vGFrom.y - pVG->bbox.bll.y) / pVG->GetVoxSizeY();
   vGFrom.z = (vGFrom.z - pVG->bbox.bll.z) / pVG->GetVoxSizeZ();
   vGTo.x   = (vGTo.x -   pVG->bbox.bll.x) / pVG->GetVoxSizeX();
   vGTo.y   = (vGTo.y -   pVG->bbox.bll.y) / pVG->GetVoxSizeY();
   vGTo.z   = (vGTo.z -   pVG->bbox.bll.z) / pVG->GetVoxSizeZ();

   // Clamp indices
   vGFrom.x = _m3dClamp(0.f, pVG->GetNVoxelX() * (1.f - M3D_EPSILON), vGFrom.x);
   vGFrom.y = _m3dClamp(0.f, pVG->GetNVoxelY() * (1.f - M3D_EPSILON), vGFrom.y);
   vGFrom.z = _m3dClamp(0.f, pVG->GetNVoxelZ() * (1.f - M3D_EPSILON), vGFrom.z);
   vGTo.x   = _m3dClamp(0.f, pVG->GetNVoxelX() * (1.f - M3D_EPSILON), vGTo.x);
   vGTo.y   = _m3dClamp(0.f, pVG->GetNVoxelY() * (1.f - M3D_EPSILON), vGTo.y);
   vGTo.z   = _m3dClamp(0.f, pVG->GetNVoxelZ() * (1.f - M3D_EPSILON), vGTo.z);

   // we launch imaginary ray from starting point to finish one and store some parameters of this ray
   // in moment 0 our ray is in starting point and in moment 1 is in finishing
   // 
   // dvX: voxel index increment
   // dtX: time to cross arbitrary layer of unit width {x | x0 <= x < x0 + 1, x0 is real}
   // tX:  time to go from starting point to another layer {x | x0 <= x < x0 + 1, x0 is integer}
   //
   // dvY, dtY, tY, dvZ, dtZ, tZ have the same definitions
   if (m3dIsZero(vGFrom.x - vGTo.x)) {
      vGTo.x = vGFrom.x; // make approximate equality to be exact
      tX  = M3D_INFINITY;     // ray never leaves starting layer
      dvX = 0;
      ctX = M3D_INFINITY;
   } else {
      if (vGFrom.x < vGTo.x) {
         dvX = 1;                                  
         dtX = 1 / (vGTo.x - vGFrom.x);            
         tX  = dtX * (1.f - m3dFraction(vGFrom.x, 1.f));
      } else {
         dvX = -1;                                 
         dtX = 1 / (vGFrom.x - vGTo.x);            
         tX = dtX * m3dFraction(vGFrom.x, 1.f);        
      }
      m3dV    vDir;
      if (!m3dGetDir(&vGFrom, &vGTo, &vDir)) {
         ASSERT(FALSE);
      }
      ctX = (vGTo.x - vGFrom.x) * pVG->GetVoxSizeX() / m3dDotProduct(&vDir, &m3dVUnitX);
   }
   if (m3dIsZero(vGFrom.y - vGTo.y)) {
      vGTo.y = vGFrom.y; // make approximate equality to be exact
      tY  = M3D_INFINITY;     // ray never leaves starting layer
      dvY = 0;
      ctY = M3D_INFINITY;
   } else {
      if (vGFrom.y < vGTo.y) {
         dvY = 1;                                  
         dtY = 1 / (vGTo.y - vGFrom.y);            
         tY = dtY * (1.f - m3dFraction(vGFrom.y, 1.f));
      } else {
         dvY = -1;                                 
         dtY = 1 / (vGFrom.y - vGTo.y);            
         tY = dtY * m3dFraction(vGFrom.y, 1.f);        
      }
      m3dV    vDir;
      if (!m3dGetDir(&vGFrom, &vGTo, &vDir)) {
         ASSERT(FALSE);
      }
      ctY = (vGTo.y - vGFrom.y) * pVG->GetVoxSizeY() / m3dDotProduct(&vDir, &m3dVUnitY);
   }
   if (m3dIsZero(vGFrom.z - vGTo.z)) {
      vGTo.z = vGFrom.z; // make approximate equality to be exact
      tZ  = M3D_INFINITY;     // ray never leaves starting layer
      dvZ = 0;
      ctZ = M3D_INFINITY;
   } else {
      if (vGFrom.z < vGTo.z) {
         dvZ = 1;                                  
         dtZ = 1 / (vGTo.z - vGFrom.z);            
         tZ = dtZ * (1.f - m3dFraction(vGFrom.z, 1.f));
      } else {
         dvZ = -1;                                 
         dtZ = 1 / (vGFrom.z - vGTo.z);            
         tZ = dtZ * m3dFraction(vGFrom.z, 1.f);        
      }
      m3dV    vDir;
      if (!m3dGetDir(&vGFrom, &vGTo, &vDir)) {
         ASSERT(FALSE);
      }
      ctZ = (vGTo.z - vGFrom.z) * pVG->GetVoxSizeZ() / m3dDotProduct(&vDir, &m3dVUnitZ);
   }

   // get start and finish voxel indices
   fromX = (int)vGFrom.x;
   fromY = (int)vGFrom.y;
   fromZ = (int)vGFrom.z;
   ASSERT(pVG->IsValidInd(fromX, fromY, fromZ));
   pVG->ClampIndices(&fromX, &fromY, &fromZ);
   ASSERT(pVG->IsValidInd(fromX, fromY, fromZ));
   toX = (int)vGTo.x;
   toY = (int)vGTo.y;
   toZ = (int)vGTo.z;
   ASSERT(pVG->IsValidInd(toX, toY, toZ));
   pVG->ClampIndices(&toX, &toY, &toZ);
   ASSERT(pVG->IsValidInd(toX, toY, toZ));

   // number of voxel in list (manhattan distance between centers of starting and finishing voxels)
   nVoxels = abs(toX - fromX) + abs(toY - fromY) + abs(toZ - fromZ) + 1;

   // starting voxel
   vX = fromX;
   vY = fromY;
   vZ = fromZ;
   i  = 0;
   distCur = 0.f;

   // if last layer is already reached disable further movement in given direction
   if (fromX == toX) tX = M3D_INFINITY;
   if (fromY == toY) tY = M3D_INFINITY;
   if (fromZ == toZ) tZ = M3D_INFINITY;

   return TRUE;
}
/*************************************************************
* voxITER_RAY::Next ()
*
* Make list of indices (of voxels), which can be touched 
* while the moving from vFrom to vTo.
*************************************************************/
BOOL voxITER_RAY::Next (int *indList, int *nInd, int indNumMax)
{
   ASSERT(indNumMax >= 1);

   if (i >= nVoxels) {
      return FALSE;
   }
   if (tX < tY) {
      if (tX < tZ) {
         distCur = tX * ctX;
      } else {
         distCur = tZ * ctZ;
      }
   } else {
      if (tY < tZ) {
         distCur = tY * ctY;
      } else {
         distCur = tZ * ctZ;
      }
   }

   // store voxel in list
   indList[0] = pVG->GetLinearInd(vX, vY, vZ);
   *nInd      = 1;

   // tX, tY, tZ contains times before ray hits in one of three integer planes
   // every intersection with planes means that ray leaves one voxel and comes in next
   // we need to select nearest hit and increment one of voxel's indices
   if (tX < tY) {
      if (tX < tZ) {
         // ray leaves one voxel and comes in into next
         vX += dvX;

         // if last layer is reached disable further movement in this direction
         // otherwise store moment of next intersection with plane
         tX = (vX == toX) ? M3D_INFINITY : tX + dtX;
      } else {
         // ray leaves one voxel and comes in into next
         vZ += dvZ;

         // if last layer is reached disable further movement in this direction
         // otherwise store moment of next intersection with plane
         tZ = (vZ == toZ) ? M3D_INFINITY : tZ + dtZ;
      }
   } else {
      if (tY < tZ) {
         // ray leaves one voxel and comes in into next
         vY += dvY;

         // if last layer is reached disable further movement in this direction
         // otherwise store moment of next intersection with plane
         tY = (vY == toY) ? M3D_INFINITY : tY + dtY;
      } else {
         // ray leaves one voxel and comes in into next
         vZ += dvZ;

         // if last layer is reached disable further movement in this direction
         // otherwise store moment of next intersection with plane
         tZ = (vZ == toZ) ? M3D_INFINITY : tZ + dtZ;
      }
   }
   i ++;
   return TRUE;
}
/*************************************************************
* voxITER_CYL
*************************************************************/
/*************************************************************
* voxITER_CYL::SetCyl ()
*
*************************************************************/
BOOL voxITER_CYL::SetCyl(m3dV *_vFrom, m3dV *_vTo, float _radius) 
{ 
   radius = _radius; 
   if (!SetRay(_vFrom, _vTo)) {
      // TBD: check distance to the box
   }
   return TRUE;
}

/*************************************************************
* voxITER_CYL::Start ()
*
*************************************************************/
BOOL voxITER_CYL::Start (void)
{
   if (!voxITER_RAY::Start()) {
      return FALSE;
   }
/*


   m3dV   _center;

   if (!pVG->bbox.IsIsectBox(&box)) {
      return FALSE;
   }
   pVG->GetPosInd(&box.bll, &from.x, &from.y, &from.z);
   pVG->GetPosInd(&box.fur, &to.x,   &to.y,   &to.z);
   box.GetCenter(&_center);
   pVG->GetPosInd(&_center, &center.x, &center.y, &center.z);
   dist = 0;
*/
   return TRUE;
}
/*************************************************************
* voxITER_CYL::Next ()
*
*************************************************************/
BOOL voxITER_CYL::Next (int *indList, int *nInd, int indNumMax)
{
   return voxITER_RAY::Next(indList, nInd, indNumMax);
}

/*************************************************************
* voxITER_MIN_DIST
*************************************************************/
/*************************************************************
* voxITER_MIN_DIST::Start ()
*
* Start iterator of indices (of voxels), which can be touched 
* while the moving from vFrom to vTo.
*************************************************************/
BOOL voxITER_MIN_DIST::Start (void)
{
   m3dV    center;
   m3dBOX  bb;

   from.x = from.y = from.z = 0;
   to.x = pVG->nVoxel.x - 1;
   to.y = pVG->nVoxel.y - 1;
   to.z = pVG->nVoxel.z - 1;

   box.GetCenter(&center);
   _m3dLineCombine(&center, &pVG->voxSize, -0.5f+M3D_EPSILON, &bb.bll);
   _m3dLineCombine(&center, &pVG->voxSize,  0.5f-M3D_EPSILON, &bb.fur);

   pVG->GetPosInd(&bb.bll, &fromNext.x, &fromNext.y, &fromNext.z, FALSE);
   pVG->GetPosInd(&bb.fur, &toNext.x,   &toNext.y,   &toNext.z  , FALSE);

   return TRUE;
}
void voxITER_MIN_DIST::SetPoint(m3dV *_point)
{ 
   box.bll = box.fur = *_point;  
   return;
}

//
// End-of-file M3D_VOX.CPP
//
